//
// Created by buyi on 17-10-26.
//

#include <algorithm>
#include <iostream>
#include <thread>

#include <ros/ros.h>
#include "std_msgs/String.h"
#include "mnorbslam_msgs/mnorbslam.h"
#include <opencv2/core/core.hpp>
#include "include/System.h"


ORB_SLAM2::System* SLAM;
string strAssociationFilename;
string strIMagefoldFilename;
vector<float> vTimesTrack;
int nImages;

void LoadImages(const string &strAssociationFilename, vector<string> &vstrImageFilenamesRGB,
                vector<string> &vstrImageFilenamesD, vector<double> &vTimestamps);

void UpdatetoMap(const mnorbslam_msgs::mnorbslam& tmsg)
{
    SLAM->UpdatetoMap(tmsg);
}

void ProcessImage()
{
    // Retrieve paths to images
    vector<string> vstrImageFilenamesRGB;
    vector<string> vstrImageFilenamesD;
    vector<double> vTimestamps;

    LoadImages(strAssociationFilename, vstrImageFilenamesRGB, vstrImageFilenamesD, vTimestamps);

    // Check consistency in the number of images and depthmaps
    nImages = vstrImageFilenamesRGB.size();
    if(vstrImageFilenamesRGB.empty())
    {
        cerr << endl << "No images found in provided path." << endl;
        return ;
    }
    else if(vstrImageFilenamesD.size()!=vstrImageFilenamesRGB.size())
    {
        cerr << endl << "Different number of images for rgb and depth." << endl;
        return ;
    }

    // Vector for tracking time statistics
    vTimesTrack.resize(nImages);

    cout << endl << "-------" << endl;
    cout << "Start processing sequence ..." << endl;
    cout << "Images in the sequence: " << nImages << endl << endl;

    // Main loop
    cv::Mat imRGB, imD;
    for(int ni=0; ni<nImages; ni++)
    {
        // Read image and depthmap from file
        imRGB = cv::imread(strIMagefoldFilename+"/"+vstrImageFilenamesRGB[ni],CV_LOAD_IMAGE_UNCHANGED);
        imD = cv::imread(strIMagefoldFilename+"/"+vstrImageFilenamesD[ni],-1);
        double tframe = vTimestamps[ni];

        if(imRGB.empty())
        {
            cerr << endl << "Failed to load image at: "
                 << strIMagefoldFilename << "/" << vstrImageFilenamesRGB[ni] << endl;
            return ;
        }

#ifdef COMPILEDWITHC11
        std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
#else
        std::chrono::monotonic_clock::time_point t1 = std::chrono::monotonic_clock::now();
#endif

        // Pass the image to the SLAM system
        SLAM->TrackRGBD(imRGB,imD,tframe);

#ifdef COMPILEDWITHC11
        std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
#else
        std::chrono::monotonic_clock::time_point t2 = std::chrono::monotonic_clock::now();
#endif

        double ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();

        vTimesTrack[ni]=ttrack;

        // Wait to load the next frame
        double T=0;
        if(ni<nImages-1)
            T = vTimestamps[ni+1]-tframe;
        else if(ni>0)
            T = tframe-vTimestamps[ni-1];

        if(ttrack<T)
            usleep((T-ttrack)*1e6);
    }
    cout<<"done"<<endl;
    ros::shutdown();
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "Tracking");
    ros::NodeHandle n("~");

    ros::Subscriber mLMupdate_subscriber = n.subscribe("/LocalMapping/LMUpdatemsgs", 100, UpdatetoMap);
    ros::Subscriber mLCupdate_subscriber = n.subscribe("/LoopClosing/UpdateOptimizeData", 100, UpdatetoMap);

    if (argc!=5)
    {
        cerr << endl << "Usage: rosrun ORB_SLAM2 RGBD path_to_settings" << endl;
        ros::shutdown();
        return 1;
    }

    strAssociationFilename = string(argv[4]);
    strIMagefoldFilename = string(argv[3]);

    // Buyi init the service params
    int tMapFramesSize;
    n.param<int>("tMapFramesSize", tMapFramesSize, false);


    // Create SLAM system. It initializes all system threads and gets ready to process frames.
    SLAM = new ORB_SLAM2::System(argv[1], argv[2], ORB_SLAM2::System::RGBD, false, &n);

    std::thread Image_process{ProcessImage};

    ros::spin();
    // Stop all threads

    SLAM->Shutdown();
    // Tracking time statistics
    sort(vTimesTrack.begin(),vTimesTrack.end());
    float totaltime = 0;
    for(int ni=0; ni<nImages; ni++)
    {

        totaltime+=vTimesTrack[ni];
    }
    cout << "-------" << endl << endl;
    cout << "median tracking time: " << vTimesTrack[nImages/2] << endl;
    cout << "mean tracking time: " << totaltime/nImages << endl;
    // Save camera trajectory
    SLAM->Generate_PointcloudMap();
    SLAM->SaveTrajectoryTUM("CameraTrajectory.txt");
    SLAM->SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");

    return 0;


}

void LoadImages(const string &strAssociationFilename, vector<string> &vstrImageFilenamesRGB,
                vector<string> &vstrImageFilenamesD, vector<double> &vTimestamps)
{
    ifstream fAssociation;
    fAssociation.open(strAssociationFilename.c_str());
    while(!fAssociation.eof())
    {
        string s;
        getline(fAssociation,s);
        if(!s.empty())
        {
            stringstream ss;
            ss << s;
            double t;
            string sRGB, sD;

            ss >> t;
            vTimestamps.push_back(t);

            ss >> sRGB;
            vstrImageFilenamesRGB.push_back(sRGB);
            ss >> t;

            ss >> sD;
            vstrImageFilenamesD.push_back(sD);

        }
    }
}